/* **************************************************************** */
/* Authors:     Shana Van Dessel, Thomas Van Winckel & Cedric Wens  */
/* Document:    Robocup.cpp                                         */
/* Description: (Hoofdklasse) Beheren van alle ander klassen en     */
/*              algemene functies                                   */
/* **************************************************************** */

#include "Robocup.h"
#define DEBUG 1

void Robocup::initThings(){
	#ifdef DEBUG
		cout << "INIT THINGS" << endl;
    #endif

    robot.setXPos(0);
	robot.setYPos(0);
	robot.setAngle(0);
	robot.setDistance(0);
	robot.setRotation(0);
    robot.setCaught(false);

    can.setXPos(0);
	can.setYPos(0);

    home.setXPos(0);
	home.setYPos(0);

	#ifdef DEBUG
		cout << "robot(" << getRobot().getXPos() << "," << getRobot().getYPos() << "," << getRobot().getAngle() << ")"<< endl;
        cout << "doel(" << getCan().getXPos() << "," << getCan().getYPos() << ")"<< endl;
        cout << "gar(" << getHome().getXPos() << "," << getHome().getYPos() << ")"<< endl << endl;
    #endif
}

void Robocup::updateThings(Info data){
    #ifdef DEBUG
		cout << "UPDATE THINGS" << endl;
    #endif

    can.setXPos(data.doelx);
    can.setYPos(data.doely);
    home.setXPos(data.garx);
    home.setYPos(data.gary);
    robot.setXPos(data.robx);
    robot.setYPos(data.roby);
    robot.setAngle(data.robhoek);

	#ifdef DEBUG
		cout << "robot(" << getRobot().getXPos() << "," << getRobot().getYPos() << "," << getRobot().getAngle() << ")"<< endl;
        cout << "doel(" << getCan().getXPos() << "," << getCan().getYPos() << ")"<< endl;
        cout << "gar(" << getHome().getXPos() << "," << getHome().getYPos() << ")"<< endl << endl;
    #endif
}

void Robocup::prepareRobot(bool blikje){
    #ifdef DEBUG
		cout << "PREPARE ROBOT" << endl;
    #endif

    int distance = 0;
    int angle = 0;
    int dx, dy;

    if(!blikje)
    {
        dx = home.getXPos() - robot.getXPos();
        dy = home.getYPos() - robot.getYPos();

    }
    else
    {
        dx = can.getXPos() - robot.getXPos();
        dy = can.getYPos() - robot.getYPos();
    }

    distance = sqrt( pow(dx,2) + pow(dy,2));  // Afstand
    
    if(dx==0){                      // Hoek (-90, 90 )
        if(dy>0)
            angle = 90;
        else
            angle = -90;
    }
    else if(dy == 0){
        if(dx>0)
            angle = 0;
        else
            angle = 180;
    }
    
	else
	{
		angle = (int)(atan2(dy,dx) * (180 / 3.1415926535)); 	// Hoek (-90 -> 90)
	}
    
	int rot = robot.getAngle() - angle;
    
	if(rot < -180){
        rot += 360;
	}else
        if(rot > 180){
        rot -= 360;
    }
	/*if(robot.getAngle() > 0 && robot.getAngle() < 90){
        robot.setRotation(-(angle-robot.getAngle()));
	}else{
        robot.setRotation(angle+robot.getAngle());
	}*/


    #ifdef DEBUG
		cout << "distance = " << distance << ", absolute hoek = " << angle << ", robothoek = " << robot.getAngle() << ", relatieve hoek = " << rot  << endl << endl;
    #endif

    robot.whereTo(distance, rot);
}

void Robocup::startRobot(){
    #ifdef DEBUG
		cout << "START ROBOT" << endl << endl;
    #endif
    robot.connect();
}

void Robocup::stopRobot(){
    #ifdef DEBUG
		cout << "STOP ROBOT" << endl << endl;
    #endif
    robot.disconnect();
}

void Robocup::moveRobot(bool dichtbij){
    #ifdef DEBUG
		cout << "MOVE ROBOT" << endl << endl;
    #endif
	robot.moveIt(dichtbij);
}

// GETTERS EN SETTERS ---------------------------------------------
void Robocup::setCan(Thing c){
    can  = c;
}

Thing Robocup::getCan(){
    return can;
}

void Robocup::setHome(Thing h){
    home = h;
}

Thing Robocup::getHome(){
    return home;
}

void Robocup::setRobot(Robot r){
    robot = r;
}

Robot Robocup::getRobot(){
    return robot;
}

void Robocup::setReceiver(Reciever r){
    receiver = r;
}

Reciever Robocup::getReceiver(){
    return receiver;
}
